/********************************************************************************

 **** Copyright (C), 2018, Fortior Technology Co., Ltd.                      ****

 ********************************************************************************
 * File Name     : FocControl.c
 * Author        : Bruce HW&RD
 * Date          : 2018-12-27
 * Description   : .C file function description
 * Version       : 1.0
 * Function List :
 *
 * Record        :
 * 1.Date        : 2018-12-27
 *   Author      : Bruce HW&RD
 *   Modification: Created file

********************************************************************************/

#include <FU68xx_4.h>
#include <MyProject.h>

/* Private variables ----------------------------------------------------------------------------*/
MotStaType mcState;
MotStaM    McStaSet;
Timecnt      Time;


uint16 TimeCnt;

/*****************************************************************************
 * Function      : MC_Control
 * Description   : 电机控制状态机
 * Input         : void
 * Output        : None
 * Return        :
 * Others        :
 * Record
 * 1.Date        : 20190319
 *   Author      : Bruce HW&RD
 *   Modification: Created function

*****************************************************************************/
void MC_Control(void)
{
    switch (mcState)
    {
        case mcReady:
            Motor_Ready();
            
            if ((mcCurOffset.OffsetFlag == 1) && (mcSpeedRamp.FlagONOFF == 1))
            { mcState = mcInit; }
            
            break;
            
        case mcInit:
            Motor_Init();
            FOC_Init();
            mcState = mcTailWind;
            mcFocCtrl.State_Count   = 0;
            break;
            
        case mcCharge:
            Motor_Charge();
            #if (IPMState == IPMtest)               // IPM测试或者MOS测试,MCU输出固定占空比
            {
            }
            #elif (IPMState == NormalRun)           // 按正常电机状态运行
            {
                if (mcFocCtrl.State_Count == 0)
                {
                    mcState = mcTailWind;
                    MotorStateTime.TailWindOneTime = 10;
                    Time.Count  = 0;
                }
            }
            #endif
            break;
            
        case mcTailWind:
            #if (TailWind_Mode == NoTailWind)
            mcState = mcPosiCheck;
            mcFocCtrl.mcPosCheckAngle = 0xffff;
            McStaSet.SetFlag.PosiCheckSetFlag = 0;
            #elif (TailWind_Mode == TailWind)
            Motor_TailWind();
            #endif
            break;
            
        case mcPosiCheck:
            #if (PosCheckEnable == 0)
            mcState                   = mcAlign;
            mcFocCtrl.State_Count     = Align_Time;
            mcFocCtrl.mcPosCheckAngle = Align_Angle;
            #else
            DRV_ARR = (uint16)(MCU_CLOCK * 1000 * 2 / 8.0) - 1;      // 载波频率的周期值
            /*设置DRV计数器的比较匹配值，当DRV计数值与COMR相等时，根据DRV_SR寄存器的DCIM是否产生比较匹配事件*/
            DRV_COMR = ((uint16)(MCU_CLOCK * 1000 * 2 / 8.0) >> 3);
            RPD();
            #endif
            break;
            
        case mcAlign:
            Motor_Align();
            #if (AlignTestMode == 1)
            
            while (1);
            
            #else
            
            if (mcFocCtrl.State_Count == 0)
            {
                mcState = mcStart;
            }
            
            #endif
            break;
            
        case mcStart:
            Motor_Open();
            break;

        case mcRun:
            if (mcSpeedRamp.TargetValue == 0)
            {
                mcFocCtrl.mcIqref -= 10;
                FOC_IQREF = mcFocCtrl.mcIqref;
                PI3_UKH   = mcFocCtrl.mcIqref;
                //                mcState = mcBrake;  //  mcBrake
                FOC_IQREF = 0;
                mcFocCtrl.State_Count = 3000;  //3000
                mcState = mcBrake;
            }
            
            break;
            
        case mcStop:
            if ((mcFocCtrl.SpeedFlt < Motor_Min_Speed) || (mcFocCtrl.State_Count == 0))
            {
                #if (StopBrakeFlag == 0)
                {
                    if ((mcFocCtrl.mcIqref > POWER_OFF_CURRENT))
                    {
                        mcFocCtrl.mcIqref -= 5;
                        FOC_IQREF = mcFocCtrl.mcIqref;
                        PI3_UKH   = mcFocCtrl.mcIqref;
                        //                                            mcFocCtrl.State_Count = 4000;
                    }
                    else
                    {
                        mcState = mcReady;
                        MOE = 0;
                        FOC_CR1 = 0x00;
                        ClrBit(DRV_CR, FOCEN);   //关闭FOC
                        ClrBit(DRV_CR, DRVEN);
                    }
                }
                #else
                
                if (mcFocCtrl.SpeedFlt < Motor_Stop_Speed)
                {
                    ClrBit(DRV_CR, DRVEN);
                    MOE = 0;
                    FOC_CR1 = 0x00;
                    ClrBit(DRV_CR, FOCEN);
                    DRV_DR   = DRV_ARR + 1;
                    DRV_CMR &= 0xFFC0;
                    DRV_CMR |= 0x015;     // 三相下桥臂通，刹车
                    ClrBit(DRV_CR, OCS);  // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
                    SetBit(DRV_CR, DRVEN);
                    MOE = 1;
                    mcState  = mcBrake;
                    mcFocCtrl.State_Count = StopWaitTime;
                }
                
                #endif
            }
            else if (mcSpeedRamp.TargetValue > 0)
            {
                mcState = mcRun;
                mcFocCtrl.CtrlMode = 0;
                FOC_IQREF = IQ_RUN_CURRENT;
            }
            
            break;
            
        case mcBrake:
            if ((mcFocCtrl.State_Count == 0) || (mcFocCtrl.SpeedFlt < Motor_Stop_Speed))
            {
                mcState = mcReady;
                ClrBit(DRV_CR, DRVEN);
                MOE = 0;
                ClrBit(DRV_CR, FOCEN);
            }
            
            break;
            
        case mcFault:
            _nop_();
            break;
            
        default:
            break;
    }
}

